#include "Integrator.hpp"

using namespace klasPhysics;

// Applies forces
void Integrator::ApplyObjectForces(Derivative &output, ForceList *forces)
{
	for(list<Vector3D>::iterator it = forces->directional.begin(); it != forces->directional.end(); ++it)
		output.force += *it;

	for(list<Vector3D>::iterator it = forces->torque.begin(); it != forces->torque.end(); ++it)
		output.torque += *it;
}

/// Evaluate all derivative values for the physics state at time t.
/// @param state the physics state of the cube.

Derivative Integrator::Evaluate(const State &state, Real t, ForceList *forces)
{
	Derivative output;
	output.velocity = state.velocity;
	output.spin = state.spin;
	ApplyObjectForces(output, forces);

	return output;
}
	
/// Evaluate derivative values for the physics state at future time t+dt 
/// using the specified set of derivatives to advance dt seconds from the 
/// specified physics state.

Derivative Integrator::Evaluate(State state, Real t, Real dt, const Derivative &derivative, ForceList *forces)
{
	// increment the state via euler integration step
	state.location += derivative.velocity * dt;
	state.momentum += derivative.force * dt;
	state.orientation = state.orientation + derivative.spin * dt;
	state.angularMomentum += derivative.torque * dt;
	state.Recalculate();
	
	// get the new derivative at 
	Derivative output;
	output.velocity = state.velocity;
	output.spin = state.spin;
	ApplyObjectForces(output, forces);

	return output;
}

/// Integrate physics state forward by dt seconds.
/// Uses an RK4 integrator to numerically integrate with error O(5).
/// The force list is also cleared.
void Integrator::Integrate(State &state, Real t, Real dt, ForceList *forces)
{
	// Use euler integeration on four times between t and dt
	Derivative a = Evaluate(state, t, forces);
	Derivative b = Evaluate(state, t, dt*0.5f, a, forces);
	Derivative c = Evaluate(state, t, dt*0.5f, b, forces);
	Derivative d = Evaluate(state, t, dt, c, forces);
	
	// Calculate the final state based upon those four derivative
	state.location += (1.0/6.0 * dt * (a.velocity + 2.0*(b.velocity + c.velocity) + d.velocity));
	state.momentum += 1.0/6.0 * dt * (a.force + 2.0*(b.force + c.force) + d.force);
	state.orientation += 1.0/6.0 * dt * (a.spin + 2.0*(b.spin + c.spin) + d.spin);
	state.angularMomentum += 1.0/6.0 * dt * (a.torque + 2.0f*(b.torque + c.torque) + d.torque);

	state.Recalculate();
	forces->Clear();
}	